#include <iostream>
#include <thread>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/random_sample.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>


std::string dest_path = "/home/w/PROJECT/HandRegist/src/data/pcd/map1031-10cm.ply";
// std::string dest_path = "/home/w/lidar3d/src/lidar3d/data/dianwang/global.pcd";
std::string src_path = "/home/w/PROJECT/HandRegist/src/data/pcd/avia.pcd";
char cmd;
Eigen::Matrix4d T(Eigen::Matrix4d::Identity());
double eulerAngleX, eulerAngleY, eulerAngleZ;
double angle_step = 0.01;
double trans_step = 0.05;
float leafSize_src = 0.3f;
float leafSize_dest = 0.2f;

ros::Publisher dest_pub;
ros::Publisher trans_pub;
ros::Publisher src_pub;

pcl::PointCloud<pcl::PointXYZ>::Ptr dest;
pcl::PointCloud<pcl::PointXYZ>::Ptr src;
pcl::PointCloud<pcl::PointXYZ>::Ptr dest_origin;
pcl::PointCloud<pcl::PointXYZ>::Ptr src_origin;


void user_control()
{

    cv::Mat UI = cv::Mat::zeros(200, 200, CV_8UC1);
    cv::imshow("User Input", UI);
    // eulerAngleX = eulerAngleY = eulerAngleZ = 0;
    cmd = 0;
    bool flag_cloud_need_move = true;
    bool flag_show_newpoints = false;

    while (true)
    {
        flag_cloud_need_move = true;
        flag_show_newpoints = false;
        cmd = cv::waitKey();
        switch (cmd)
        {
            case 'R':   // roll(x)+
                eulerAngleX += angle_step;
            break;
            case 'P':   // pitch(y)+
                eulerAngleY += angle_step;
            break;
            case 'W':   // yaw(z)+
                eulerAngleZ += angle_step;
            break;
            case 'r':   // roll(x)-
                eulerAngleX -= angle_step;
            break;
            case 'p':   // pitch(y)-
                eulerAngleY -= angle_step;
            break;
            case 'w':   // yaw(z)-
                eulerAngleZ -= angle_step;
            break;


            case 'X':   // x+
                T(0, 3) += trans_step;
            break;
            case 'Y':   // y+
                T(1, 3) += trans_step;
            break;
            case 'Z':   // z+
                T(2, 3) += trans_step;
            break;
            case 'x':   // x-
                T(0, 3) -= trans_step;
            break;
            case 'y':   // y-
                T(1, 3) -= trans_step;
            break;
            case 'z':   // z-
                T(2, 3) -= trans_step;
            break;


            case 'A':   //angle_step+ 
                angle_step *= 10;
                printf("angle_step: %f rad\n", angle_step);
                flag_cloud_need_move = false;
            break;
            case 'a':   //angle_step- 
                angle_step /= 10;
                printf("angle_step: %f rad\n", angle_step);
                flag_cloud_need_move = false;
            break;
            case 'T':   //trans_step+ 
                trans_step *= 10;
                printf("trans_step: %f m\n", trans_step);
                flag_cloud_need_move = false;
            break;
            case 't':   //trans_step- 
                trans_step /= 10;
                printf("trans_step: %f m\n", trans_step);
                flag_cloud_need_move = false;
            break;

            case 's':   //search and show the new points
                flag_cloud_need_move = false;
                flag_show_newpoints = true;
            break;

            default:
                flag_cloud_need_move = false;
            break;
        }

        if (cmd == 'q')
            break;

        if (flag_cloud_need_move)
        {
            Eigen::AngleAxisd roll_angle(eulerAngleX, Eigen::Vector3d::UnitX());
            Eigen::AngleAxisd pitch_angle(eulerAngleY, Eigen::Vector3d::UnitY());
            Eigen::AngleAxisd yaw_angle(eulerAngleZ, Eigen::Vector3d::UnitZ());
            Eigen::Matrix3d R;
            R = yaw_angle * pitch_angle * roll_angle;
            T.topLeftCorner<3, 3>() = R;
            std::cout << T << std::endl;
            pcl::PointCloud<pcl::PointXYZ> src_trans;
            src_trans.clear();
            for (int i = 0; i < src->size(); i+=10)
            {
                Eigen::Vector3d src_point(src->points[i].x, src->points[i].y, src->points[i].z);
                src_point = R * src_point + T.topRightCorner<3, 1>();
                pcl::PointXYZ src_trans_point(src_point[0], src_point[1], src_point[2]);
                src_trans.push_back(src_trans_point);
            }
            sensor_msgs::PointCloud2 msg_trans;
            pcl::toROSMsg(src_trans, msg_trans);
            msg_trans.header.frame_id = "map";
            trans_pub.publish(msg_trans);

            static int cnt = 0;
            if (cnt < 5) {
                sensor_msgs::PointCloud2 msg_dest;
                pcl::toROSMsg(*dest, msg_dest);
                msg_dest.header.frame_id = "map";
                dest_pub.publish(msg_dest);
                // printf("pcd published.\n");
                cnt++;
            }

        }

        if (flag_show_newpoints)
        {
            pcl::KdTreeFLANN<pcl::PointXYZ> my_kdtree;
            my_kdtree.setInputCloud(dest);
            pcl::PointCloud<pcl::PointXYZ> src_new;
            src_new.clear();
            for (int i = 0; i < src_origin->size(); i++)
            {
                Eigen::Vector3d src_point(src_origin->points[i].x, src_origin->points[i].y, src_origin->points[i].z);
                src_point = T.topLeftCorner<3, 3>() * src_point + T.topRightCorner<3, 1>();
                if (src_point[2] > 3)   //above 3m
                {
                    pcl::PointXYZ search_point(src_point[0], src_point[1], src_point[2]);
                    std::vector<int> indices;
                    std::vector<float> distances;
                    int num_neighbors = my_kdtree.radiusSearch(search_point, 0.2, indices, distances);
                    if (num_neighbors < 1)
                    {
                        src_new.push_back(search_point);
                    }
                }
            }
            if (!src_new.empty())
            {
                sensor_msgs::PointCloud2 msg_new;
                pcl::toROSMsg(src_new, msg_new);
                msg_new.header.frame_id = "map";
                trans_pub.publish(msg_new);
                printf("new points published. point num: %d\n", src_new.size());

                pcl::PointCloud<pcl::PointXYZ>::Ptr save(new pcl::PointCloud<pcl::PointXYZ>);
                *save = *dest_origin + src_new;
                // pcl::io::savePCDFile("/home/w/HandRegist/src/HandRegist/newmap.pcd", *save);
                printf("pcd saved.\n");
            }
            else
                printf("no enough points.\n");
            std::cout << "T:" << T << std::endl;
        }
    }
}

int main(int argc, char** argv) {

    dest.reset(new pcl::PointCloud<pcl::PointXYZ>);
    src.reset(new pcl::PointCloud<pcl::PointXYZ>);
    dest_origin.reset(new pcl::PointCloud<pcl::PointXYZ>);
    src_origin.reset(new pcl::PointCloud<pcl::PointXYZ>);
    // if (pcl::io::loadPCDFile<pcl::PointXYZ>(dest_path, *dest_origin) == -1) {
    //     ROS_ERROR("Couldn't read the destination cloud.\n");
    //     return -1;
    // }
    // if (pcl::io::loadPCDFile<pcl::PointXYZ>(src_path, *src_origin) == -1) {
    //     ROS_ERROR("Couldn't read the source cloud.\n");
    //     return -1;
    // }

    if (pcl::io::loadPLYFile(dest_path, *dest) == -1) {
        ROS_ERROR("Couldn't read the destination cloud.\n");
        return -1;
    }
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(src_path, *src) == -1) {
        ROS_ERROR("Couldn't read the source cloud.\n");
        return -1;
    }

    printf("pcd loaded.\n");


    // printf("src points: %d.\n", src_origin->size());
    // pcl::VoxelGrid<pcl::PointXYZ> voxel;
    // // voxel.setInputCloud(src_origin);
    // // voxel.setLeafSize(leafSize_src, leafSize_src, leafSize_src);
    // // voxel.filter(*src);
    // // printf("src points after filter: %d.\n", src->size());
    // pcl::RandomSample<pcl::PointXYZ> sample;
    // sample.setInputCloud(src_origin);
    // int indices = (int)(0.1 * src_origin->size());
    // sample.setSample(indices);
    // sample.filter(*src);
    // printf("src points after filter: %d.\n", src->size());

    // printf("dest points: %d.\n", dest_origin->size());
    // voxel.setInputCloud(dest_origin);
    // voxel.setLeafSize(leafSize_dest, leafSize_dest, leafSize_dest);
    // voxel.filter(*dest);
    // printf("dest points after filter: %d.\n", dest->size());



    ros::init(argc, argv, "hand_regist");
    ros::NodeHandle nh;
    dest_pub = nh.advertise<sensor_msgs::PointCloud2>("/cloud_dest", 1);
    trans_pub = nh.advertise<sensor_msgs::PointCloud2>("/cloud_trans", 1);
    src_pub = nh.advertise<sensor_msgs::PointCloud2>("/cloud_src", 1);

    // T << 1, 0, 0, 0, \
    //      0, 1, 0, 0, \
    //      0, 0, 1, 0, \
    //      0, 0, 0, 1;
    T <<    0.437299,  -0.120279,   0.891237,   -456.245,
    0.8943, -0.0463692,  -0.445059,      1.955,
 0.0948571,   0.991657,   0.087288,       2.07,
         0,          0,          0,          1;


    // T << -0.872539, -0.402918,  0.276284,  -1.79,  \
    //      0.358771 , -0.912309,  -0.197422, -60.25, \
    //      0.331601 , -0.0731359, 0.940581,  1.715,  \
    //      0        , 0        ,  0       ,  1;

    // T << -0.629516, -0.717681, -0.297731,     39.26,\
    //     -0.697231,  0.690885, -0.191171,     -9.45,\
    //     0.342898,  0.087242, -0.935313,    -0.385,\
    //             0,         0,         0,         1;

    // T << -0.640756, -0.714703, -0.280414,     40.91,\
    //     -0.695568,  0.695019, -0.182028,     -9.45,\
    //     0.324989, 0.0784108, -0.942462,     2.015,\
    //             0,         0,         0,         1;

    // T << 0.0214819,   0.999672,  0.0139118,     113.01,\
    //     -0.994571,   0.019951,   0.102134,      -37.1,\
    //     0.101823, -0.0160303,   0.994673,      2.065,\
    //             0,          0,          0,          1;




    // T <<-0.870664,   -0.40541,   0.278545,      -1.84,\
    //     0.361568,  -0.911431,  -0.196375,     -60.45,\
    //     0.333487, -0.0702641,   0.940133,      1.665,\
    //             0,          0,          0,          1;

    eulerAngleX = T.topLeftCorner<3, 3>().eulerAngles(2, 1, 0)[2];
    eulerAngleY = T.topLeftCorner<3, 3>().eulerAngles(2, 1, 0)[1];
    eulerAngleZ = T.topLeftCorner<3, 3>().eulerAngles(2, 1, 0)[0];

    std::cout << eulerAngleX << " " << eulerAngleY << " " << eulerAngleZ << std::endl;


    std::thread control_thread(&user_control);
    control_thread.detach();

    // for (int i = 0; i < 5; i++)
    // {
    //     sensor_msgs::PointCloud2 msg_dest;
    //     pcl::toROSMsg(*dest, msg_dest);
    //     msg_dest.header.frame_id = "map";
    //     dest_pub.publish(msg_dest);
    // }
    // printf("pcd published.\n");

    ros::spin();


    return 0;
}